/**
 * 程序功能：实现参数服务器的作用，相关参数可以通过客户端或者终端进行更改
 * 
 * 编译：colcon build --packages-select param_example
 * 环境配置：source ./install/setup.bash
 * 运行：ros2 run param_example simple_param
 * 
 * 终端查看节点参数信息：ros2 param list
 * 终端交互：另起一个终端输入如：ros2 param set /simple_param k 20.0
 * 
 * add_on_set_parameters_callback 回调：这个回调函数是为了在参数被更新时触发。
 * 它相当于为参数管理服务添加一个“监听器”，用于响应外部对参数的更新操作(通常来自于客户端请求，比如ros2 param set命令)
 */

#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/parameter.hpp"

class MyNode : public rclcpp::Node
{
public:
    MyNode() : Node("simple_param")
    {
        // 声明参数并设置默认值
        this->declare_parameter("k", 0.0);
        this->declare_parameter("max_speed", 10.0);

        // 注册一个参数回调函数，当参数更新时会触发该函数
        parameters_callback_handle_ = this->add_on_set_parameters_callback(
            std::bind(&MyNode::on_set_parameters, this, std::placeholders::_1));
    }

private:
    double k_;         // 存储 k 参数的值
    double max_speed_; // 存储max_speed 参数的值
    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;

    // 参数更新回调函数
    rcl_interfaces::msg::SetParametersResult on_set_parameters(const std::vector<rclcpp::Parameter> &params)
    {
        for (const auto &param : params)
        {
            if (param.get_name() == "k")
            {
                k_ = param.as_double();
                RCLCPP_INFO(this->get_logger(), "参数 %s 更新为： %f", param.get_name().c_str(), param.as_double());
            }
            else if (param.get_name() == "max_speed")
            {
                max_speed_ = param.as_double();
                RCLCPP_INFO(this->get_logger(), "参数 %s 更新为： %f", param.get_name().c_str(), param.as_double());
            }
        }
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        return result;
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    // 创建并运行MyNode节点
    rclcpp::spin(std::make_shared<MyNode>());

    rclcpp::shutdown();
    return 0;
}
